/**
 * @format
 * @Author: TC
 * @Date: 2023-08-21 12:03:03
 * @Last Modified by: TC
 * @Last Modified time: 2025-05-25 20:46:02
 */

import { app } from "../../App";
import { IV2 } from "../../Interface/BaseInterface";
import { AStarPathFinding } from "./AStarPathFinding";
import { IFunnelMouth, INavCoord, IPathFindingResult } from "./NavInterface";
import { mNavMesh } from "./NavMeshConst";
import { PolygonUnit } from "./PolygonUint";

/**
 * 最终导航路线
 */
export class FinalPath {
    private _aStar: AStarPathFinding = null;
    private end: IV2;
    private start: IV2;
    /**当前漏斗起点 */
    private funnelPos: IV2;
    /**路径点 */
    private posLst: IV2[] = [];
    private areaLst: PolygonUnit[] = [];

    private _polygonIndexs: number[] = [];
    private leftConnerLst: number[] = [];
    private rightConnerLst: number[] = [];
    private leftCheckDir: IV2;
    private rightCheckDir: IV2;
    private leftLimitDir: IV2;
    private rightLimitDir: IV2;
    private leftLimitIndex: number;
    private rightLimitIndex: number;
    private leftCheckIndex: number;
    private rightCheckIndex: number;

    private _findCompleted: boolean = false;

    constructor() {
        this._aStar = new AStarPathFinding();
    }

    /**
     * 根据网格索引获取最终路径
     * @param posStart
     * @param posEnd
     * @returns
     */
    public getPathFindingResust(posStart: IV2, posEnd: IV2): IPathFindingResult {
        // tApp.log.time("导航")
        this.reset();
        this.start = posStart;
        this.end = posEnd;
        let polygonIndexs = this._aStar.getMeshIndexs(posStart, posEnd);
        if (0 == polygonIndexs.length) {
            let res: IPathFindingResult = {
                meshIndexs: [],
                path: [],
            };
            return res;
        }
        this.restartFindPath(posStart, posEnd);
        this.removeRepetitiveDots();
        let res: IPathFindingResult = {
            meshIndexs: polygonIndexs,
            path: this.posLst,
        };
        // tApp.log.timeEnd("导航")
        return res;
    }

    private restartFindPath(posStart: IV2, posEnd: IV2): IV2[] {
        if (this._findCompleted) return this.posLst;
        this.start = {
            x: posStart.x,
            y: posStart.y,
        };
        this.areaLst = [];
        let polygonIndexs = this._aStar.getMeshIndexs(posStart, posEnd);
        this._polygonIndexs = polygonIndexs;
        polygonIndexs.forEach((index) => {
            this.areaLst.push(app.navMeshMgr.polygonList[index]);
        });
        return this.CalcFunnelConnerPath();
    }

    private reset(): void {
        this._findCompleted = false;

        this.posLst = [];
        this.areaLst = [];

        this._polygonIndexs = [];
        this.leftConnerLst = [];
        this.rightConnerLst = [];
        this.leftCheckDir = { x: 0, y: 0 };
        this.rightCheckDir = { x: 0, y: 0 };
        this.leftLimitDir = { x: 0, y: 0 };
        this.rightLimitDir = { x: 0, y: 0 };
        this.leftLimitIndex = -1;
        this.rightLimitIndex = -1;
        this.leftCheckIndex = -1;
        this.rightCheckIndex = -1;
    }

    private logAllData() {
        return;
        app.log.log("当前漏斗顶点--", this.funnelPos.x, this.funnelPos.y);
        app.log.log("当前极限点--", this.leftLimitIndex, this.rightLimitIndex);
        app.log.log("当前检测点--", this.leftCheckIndex, this.rightCheckIndex);
    }

    /**
     * 根据网格获取路径
     * @param polygonIndexs //多边形索引列表
     * @returns
     */
    private CalcFunnelConnerPath(): IV2[] {
        this.pushPathDot({
            x: this.start.x,
            y: this.start.y,
        });
        this.funnelPos = {
            x: this.start.x,
            y: this.start.y,
        };
        this.leftConnerLst = [];
        this.rightConnerLst = [];

        //Init funnel
        let initIndex = this.CalcInitAreaID(this.areaLst);
        if (initIndex == -1) {
            this.pushPathDot({
                x: this.end.x,
                y: this.end.y,
            });
            this._findCompleted = true;
            return this.posLst;
        }
        //this.LogCyan($"initAreaID:{areaLst[initIndex].areaID}");

        let leftFSE: mNavMesh.FUNNEL_TYPE, rightFSE: mNavMesh.FUNNEL_TYPE;
        for (let i = initIndex + 1; i < this.areaLst.length; i++) {
            let area = this.areaLst[i];
            if (i == this.areaLst.length - 1) {
                let endVector: IV2 = {
                    x: this.end.x - this.funnelPos.x,
                    y: this.end.y - this.funnelPos.y,
                };
                this.leftCheckDir = endVector;
                this.rightCheckDir = endVector;

                let inLeft = app.mMath.cross(this.leftLimitDir, endVector) > 0;
                let inRight = app.mMath.cross(this.rightLimitDir, endVector) < 0;

                if (inLeft && !this._findCompleted) {
                    this.CalcEndConner(this.leftConnerLst, this.leftLimitIndex, this.leftLimitDir, this.end);
                    this._findCompleted = true;
                } else if (inRight && !this._findCompleted) {
                    this.CalcEndConner(this.rightConnerLst, this.rightLimitIndex, this.rightLimitDir, this.end);
                    this._findCompleted = true;
                } else {
                    this.pushPathDot(this.end);
                    this._findCompleted = true;
                }
            } else {
                //calc CheckIndex and CheckDir
                this.CalcCheckFunnel(area);
                leftFSE = this.CalcLeftFunnelChange();
                rightFSE = this.CalcRightFunnelChange();
                if (leftFSE == mNavMesh.FUNNEL_TYPE.LEFT_OUT) {
                    // tApp.log.warn("左侧外扩")
                    if (this.leftConnerLst.indexOf(this.leftCheckIndex) < 0) {
                        this.leftConnerLst.push(this.leftCheckIndex);
                    }
                }
                if (rightFSE == mNavMesh.FUNNEL_TYPE.RIGHT_OUT) {
                    // tApp.log.warn("右侧外扩")
                    if (this.rightConnerLst.indexOf(this.rightCheckIndex) < 0) {
                        this.rightConnerLst.push(this.rightCheckIndex);
                    }
                }

                switch (leftFSE) {
                    case mNavMesh.FUNNEL_TYPE.NONE:
                        // tApp.log.warn("左侧重叠--", this.leftCheckIndex)
                        this.leftLimitIndex = this.leftCheckIndex;
                        // this.logAllData();
                        break;
                    case mNavMesh.FUNNEL_TYPE.LEFT_IN:
                        // tApp.log.warn("左侧内缩--", this.leftCheckIndex)
                        this.leftLimitIndex = this.leftCheckIndex;
                        this.leftLimitDir = this.leftCheckDir;
                        this.leftConnerLst = [];
                        // this.logAllData();
                        break;
                    case mNavMesh.FUNNEL_TYPE.LEFT_CROSS:
                        // tApp.log.warn("左侧越界--")
                        this.CalcLeftToRightLimitIndex();
                        // this.logAllData();
                        return;
                        break;
                    default:
                        break;
                }

                switch (rightFSE) {
                    case mNavMesh.FUNNEL_TYPE.NONE:
                        // tApp.log.warn("右侧重叠--", this.rightCheckIndex)
                        this.rightLimitIndex = this.rightCheckIndex;
                        // this.logAllData();
                        break;
                    case mNavMesh.FUNNEL_TYPE.RIGHT_IN:
                        // tApp.log.warn("右侧内缩--", this.rightCheckIndex)
                        this.rightLimitIndex = this.rightCheckIndex;
                        this.rightLimitDir = this.rightCheckDir;
                        this.rightConnerLst = [];
                        // this.logAllData();
                        break;
                    case mNavMesh.FUNNEL_TYPE.RIGHT_CROSS:
                        // tApp.log.warn("右侧越界--")
                        this.CalcRightToLeftLimitIndex();
                        // this.logAllData();
                        return;
                        break;
                    default:
                        break;
                }
                this.logAllData();
            }
        }

        return this.posLst;
    }

    private CalcInitAreaID(areaLst: PolygonUnit[]): number {
        let initAreaID = -1;
        if (0 == areaLst.length) {
            return initAreaID;
        }

        for (let i = 0; i < areaLst.length; i++) {
            if (this.IsFunnelInitArea(areaLst[i])) {
                initAreaID = i;
                break;
            }
        }

        return initAreaID;
    }

    private IsFunnelInitArea(area: PolygonUnit): boolean {
        let index = this._polygonIndexs.indexOf(area.polygon.index);
        let leftAndRight = this.getLeftAndRightCoord(index, index + 1);
        if (!leftAndRight) {
            return false;
        }

        let left = leftAndRight.left;
        let right = leftAndRight.right;
        let v1 = {
            x: app.navMeshMgr.dotList[left.index].x - this.funnelPos.x,
            y: app.navMeshMgr.dotList[left.index].y - this.funnelPos.y,
        };
        let v2 = {
            x: app.navMeshMgr.dotList[right.index].x - this.funnelPos.x,
            y: app.navMeshMgr.dotList[right.index].y - this.funnelPos.y,
        };

        let cross = app.mMath.cross(v1, v2);
        if (cross != 0) {
            this.leftLimitIndex = left.index;
            this.rightLimitIndex = right.index;
            this.leftLimitDir = v1;
            this.rightLimitDir = v2;
            return true;
        } else {
            return false;
        }
    }

    private CalcCheckFunnel(area: PolygonUnit) {
        let index = this._polygonIndexs.indexOf(area.polygon.index);
        let leftAndRight = this.getLeftAndRightCoord(index, index + 1);
        let checkIndex1 = leftAndRight.left.index;
        let checkIndex2 = leftAndRight.right.index;
        let checkV1: IV2 = {
            x: app.navMeshMgr.dotList[checkIndex1].x - this.funnelPos.x,
            y: app.navMeshMgr.dotList[checkIndex1].y - this.funnelPos.y,
        };
        let checkV2: IV2 = {
            x: app.navMeshMgr.dotList[checkIndex2].x - this.funnelPos.x,
            y: app.navMeshMgr.dotList[checkIndex2].y - this.funnelPos.y,
        };
        this.leftCheckIndex = checkIndex1;
        this.leftCheckDir = checkV1;
        this.rightCheckIndex = checkIndex2;
        this.rightCheckDir = checkV2;

        if (0 == app.mMath.length(this.leftLimitDir.x, this.leftLimitDir.y)) this.leftLimitDir = this.leftCheckDir;
        if (0 == app.mMath.length(this.rightLimitDir.x, this.leftLimitDir.y)) this.rightLimitDir = this.rightCheckDir;
    }

    private CalcLeftFunnelChange(): mNavMesh.FUNNEL_TYPE {
        let leftFSE: mNavMesh.FUNNEL_TYPE;
        let ll = app.mMath.cross(this.leftLimitDir, this.leftCheckDir);
        if (ll > 0) {
            leftFSE = mNavMesh.FUNNEL_TYPE.LEFT_OUT;
        } else if (ll == 0) {
            leftFSE = mNavMesh.FUNNEL_TYPE.NONE;
        } else {
            let lr = app.mMath.cross(this.rightLimitDir, this.leftCheckDir);
            if (lr > 0) {
                leftFSE = mNavMesh.FUNNEL_TYPE.LEFT_IN;
            } else {
                leftFSE = mNavMesh.FUNNEL_TYPE.LEFT_CROSS;
            }
        }
        return leftFSE;
    }

    private CalcRightFunnelChange(): mNavMesh.FUNNEL_TYPE {
        let rightFSE: mNavMesh.FUNNEL_TYPE;
        let rr = app.mMath.cross(this.rightLimitDir, this.rightCheckDir);
        if (rr < 0) {
            rightFSE = mNavMesh.FUNNEL_TYPE.RIGHT_OUT;
        } else if (rr == 0) {
            rightFSE = mNavMesh.FUNNEL_TYPE.NONE;
        } else {
            let rl = app.mMath.cross(this.leftLimitDir, this.rightCheckDir);
            if (rl < 0) {
                rightFSE = mNavMesh.FUNNEL_TYPE.RIGHT_IN;
            } else {
                rightFSE = mNavMesh.FUNNEL_TYPE.RIGHT_CROSS;
            }
        }
        return rightFSE;
    }

    private CalcLeftToRightLimitIndex() {
        this.funnelPos = {
            x: app.navMeshMgr.dotList[this.rightLimitIndex].x,
            y: app.navMeshMgr.dotList[this.rightLimitIndex].y,
        };
        this.pushPathDot({
            x: app.navMeshMgr.dotList[this.rightLimitIndex].x,
            y: app.navMeshMgr.dotList[this.rightLimitIndex].y,
        });
        // tApp.log.warn("漏斗坐标修正--", this.funnelPos.x, this.funnelPos.y);

        this.restartFindPath(this.funnelPos, this.end);
    }
    private CalcRightToLeftLimitIndex() {
        this.funnelPos = {
            x: app.navMeshMgr.dotList[this.leftLimitIndex].x,
            y: app.navMeshMgr.dotList[this.leftLimitIndex].y,
        };
        this.pushPathDot({
            x: app.navMeshMgr.dotList[this.leftLimitIndex].x,
            y: app.navMeshMgr.dotList[this.leftLimitIndex].y,
        });
        // tApp.log.warn("漏斗坐标修正--", this.funnelPos.x, this.funnelPos.y);

        this.restartFindPath(this.funnelPos, this.end);
    }

    private CalcEndConner(cornerIndexLst: number[], limitIndex: number, limitDir: IV2, end: IV2) {
        // tApp.log.log("进入目标区域", cornerIndexLst, limitIndex);
        this.funnelPos = {
            x: app.navMeshMgr.dotList[limitIndex].x,
            y: app.navMeshMgr.dotList[limitIndex].y,
        };
        this.pushPathDot({
            x: this.funnelPos.x,
            y: this.funnelPos.y,
        });
        let cornerPosLst: IV2[] = [];
        for (let i = 0; i < cornerIndexLst.length; i++) {
            cornerPosLst.push(app.navMeshMgr.dotList[cornerIndexLst[i]]);
        }
        let isExist = false;
        for (let i = 0; i < cornerPosLst.length; i++) {
            if (cornerPosLst[i] == end) {
                isExist = true;
                break;
            }
        }
        if (!isExist) {
            cornerPosLst.push(end);
            // this.pushPathDot(end);
            this._findCompleted = true;
            // return;
        }

        let ln = app.mMath.normalize(limitDir);
        let connerIndex = 0;
        while (connerIndex < cornerPosLst.length) {
            let rad = Number.MAX_VALUE;
            for (let j = connerIndex; j < cornerPosLst.length; j++) {
                let checkVec: IV2 = {
                    x: cornerPosLst[j].x - this.funnelPos.x,
                    y: cornerPosLst[j].y - this.funnelPos.y,
                };
                let ckn = app.mMath.normalize(checkVec);
                let curRad = Math.abs(app.mMath.getAngle(ln, ckn));
                if (curRad < rad) {
                    rad = curRad;
                    connerIndex = j;
                }
            }
            this.funnelPos = {
                x: cornerPosLst[connerIndex].x,
                y: cornerPosLst[connerIndex].y,
            };
            this.pushPathDot(cornerPosLst[connerIndex]);
            if (cornerPosLst[connerIndex] === end) {
                this._findCompleted = true;
            }
            connerIndex++;
        }
    }

    /**
     * 获取漏斗开口左右顶点信息
     * @param currIndex 当前网格索引
     * @param nextIndex 下一网格索引
     */
    private getLeftAndRightCoord(currIndex: number, nextIndex: number): IFunnelMouth {
        let currPolygon = this.areaLst[currIndex];
        let nextPolygon = this.areaLst[nextIndex];
        if (!nextPolygon) return null;
        let center = currPolygon.getCenter();
        let publicCoords = currPolygon.getPublicCoords(nextPolygon);

        let iMouth: IFunnelMouth = {
            left: undefined,
            right: undefined,
        };

        let cross = app.mMath.cross(
            { x: publicCoords[1].x - center.x, y: publicCoords[1].y - center.y },
            { x: publicCoords[0].x - center.x, y: publicCoords[0].y - center.y }
        );

        if (cross > 0) {
            iMouth.left = publicCoords[0];
            iMouth.right = publicCoords[1];
        } else {
            iMouth.left = publicCoords[1];
            iMouth.right = publicCoords[0];
        }

        return iMouth;
    }
    private removeRepetitiveDots(): void {
        let len = this.posLst.length;
        for (let i = 0; i < this.posLst.length - 1; i++) {
            if (this.posLst[i].x === this.posLst[i + 1].x && this.posLst[i].y === this.posLst[i + 1].y) {
                this.posLst.splice(i + 1, 1);
                i--;
            }
        }
    }

    private pushPathDot(v: IV2): void {
        this.posLst.push(v);
        // tApp.log.error("插入路径点,", v.x, v.y);
    }

    /**
     * 顶点转换为二维坐标
     * @param c
     * @returns
     */
    private coordToV2(c: INavCoord): IV2 {
        return { x: c.x, y: c.y } as IV2;
    }
}
